#include<math.h>
#include<Eigen/Dense>
#include<tf/tf.h>
#include<mavros_msgs/AttitudeTarget.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include<qpOASES.hpp>

using namespace std;
USING_NAMESPACE_QPOASES

#define g_world 9.8
#define pi 3.14
#define yaw_ref 0
#define uav_mass 1.5 

#define MOTOR_P1 -0.00069
#define MOTOR_P2 0.01271
#define MOTOR_P3 -0.07948
#define MOTOR_P4 0.3052
#define MOTOR_P5 0.008775

//框架大小 3*3*1
double x_l=-1.5;
double x_h=1.5;
double y_l=-1.5;
double y_h=1.5;
double z_l=1;
double z_h=2;

int testPos(geometry_msgs::PoseStamped pos)
{
    int flag;
    double x=pos.pose.position.x;
    double y=pos.pose.position.y;
    double z=pos.pose.position.z;
    if (x<=x_h&&x>=x_l
    &&y<=y_h&&y>=y_l
    &&z<=z_h&&z>=z_l)
    {
        flag=3;
    }
    else
    {
        flag=4;
    }
    return flag;

}


 Eigen::Vector3d r_cal(Eigen::Vector3d input)
{
    Eigen::Vector3d r;
    r[0]=g_world*(input[1]*cos(yaw_ref)+input[0]*sin(yaw_ref));
    r[1]=g_world*(input[1]*sin(yaw_ref)-input[0]*cos(yaw_ref));
    r[2]=input[2]/uav_mass-g_world;
    return r;
}


mavros_msgs::AttitudeTarget output_cal(real_t u[3])
{
    mavros_msgs::AttitudeTarget output;
    geometry_msgs::Quaternion q_output;
    
    output.header.frame_id="map";
    output.header.stamp=ros::Time::now();
    output.type_mask=0b00000111;//00 000 111 
    double roll_=(u[0]*sin(yaw_ref)-u[1]*cos(yaw_ref))/g_world;//roll
    double pitch_=(u[0]*cos(yaw_ref)+u[1]*sin(yaw_ref))/g_world;//pitch
    double yaw_=0;//yaw
    q_output=tf::createQuaternionMsgFromRollPitchYaw(roll_,pitch_,yaw_);
    output.orientation=q_output;
    double thrust_=uav_mass*(u[2]+g_world)/4;//thrust
    ROS_INFO("u=%f",u[2]);
    ///thrust->throttle
    output.thrust=MOTOR_P1 * pow(thrust_,4) + MOTOR_P2 * pow(thrust_,3) + MOTOR_P3 * pow(thrust_,2) + MOTOR_P4 * thrust_ + MOTOR_P5;
    //output.thrust=thrust/30;
    ROS_INFO("thrust=%f",output.thrust);

    return output;
} 




mavros_msgs::AttitudeTarget optimization(Eigen::Vector3d input,geometry_msgs::PoseStamped pose_,geometry_msgs::TwistStamped twst)
{
    mavros_msgs::AttitudeTarget output;
    real_t sol[3];//解出值

    //K值
    double k1=12;
    double k2=13;
    //飞机位置
    double pos_x=pose_.pose.position.x;
    double pos_y=pose_.pose.position.y;
    double pos_z=pose_.pose.position.z;

    //飞行速度
    double vel_x=twst.twist.linear.x;
    double vel_y=twst.twist.linear.y;
    double vel_z=twst.twist.linear.z;

    Eigen::Vector3d input_acc;
    input_acc=r_cal(input);


    //ECBF约束条件

    real_t H[3*3]={2,0,0,
                                0,2,0,
                                0,0,2};
    real_t g_cal[3]={(-2)*input_acc[0],(-2)*input_acc[1],(-2)*input_acc[2]};
    real_t A[6*3]=
        {1,0,0,
        -1,0,0,
        0,1,0,
        0,-1,0,
        0,0,1,
        0,0,-1};
    real_t ubA[6*1]={k1*(x_h-pos_x)+k2*(-vel_x),
                                        k1*(pos_x-x_l)+k2*vel_x,
                                        k1*(y_h-pos_y)+k2*(-vel_y),
                                        k1*(pos_y-y_l)+k2*vel_y,
                                        k1*(z_h-pos_z)+k2*(-vel_z),
                                        k1*(pos_z-z_l)+k2*vel_z};
    QProblem flight(3,6);
    //Options options;
	//flight.setOptions( options );

    int_t nWSR=10;
    flight.init( H,g_cal,A,nullptr,nullptr,nullptr,ubA, nWSR);
    
    flight.getPrimalSolution(sol);
    output=output_cal(sol);

    return output;
}
